{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "4b00b24e",
   "metadata": {
    "id": "TKvYiJgnYExi"
   },
   "source": [
    "This notebook provides examples to go along with the [textbook](http://manipulation.csail.mit.edu/clutter.html).  I recommend having both windows open, side-by-side!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2735dd33",
   "metadata": {
    "id": "A4QOaw_zYLfI",
    "lines_to_end_of_cell_marker": 2
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from IPython.display import clear_output\n",
    "from pydrake.all import (\n",
    "    AbstractValue,\n",
    "    AddMultibodyPlantSceneGraph,\n",
    "    Concatenate,\n",
    "    DiagramBuilder,\n",
    "    JointSliders,\n",
    "    LeafSystem,\n",
    "    MeshcatPoseSliders,\n",
    "    MeshcatVisualizer,\n",
    "    MeshcatVisualizerParams,\n",
    "    Parser,\n",
    "    PointCloud,\n",
    "    RandomGenerator,\n",
    "    Rgba,\n",
    "    RigidTransform,\n",
    "    RotationMatrix,\n",
    "    Simulator,\n",
    "    StartMeshcat,\n",
    "    UniformlyRandomRotationMatrix,\n",
    ")\n",
    "\n",
    "from manipulation import running_as_notebook\n",
    "from manipulation.scenarios import AddFloatingRpyJoint, AddRgbdSensors, ycb\n",
    "from manipulation.utils import ConfigureParser"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a651a0ef",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start the visualizer.\n",
    "meshcat = StartMeshcat()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f320dbe5",
   "metadata": {},
   "source": [
    "# Scoring grasp candidates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4f5c002e",
   "metadata": {},
   "outputs": [],
   "source": [
    "def GraspCandidateCost(\n",
    "    diagram,\n",
    "    context,\n",
    "    cloud,\n",
    "    wsg_body_index=None,\n",
    "    plant_system_name=\"plant\",\n",
    "    scene_graph_system_name=\"scene_graph\",\n",
    "    adjust_X_G=False,\n",
    "    verbose=False,\n",
    "    meshcat_path=None,\n",
    "):\n",
    "    \"\"\"\n",
    "    Args:\n",
    "        diagram: A diagram containing a MultibodyPlant+SceneGraph that contains\n",
    "            a free body gripper and any obstacles in the environment that we\n",
    "            want to check collisions against. It should not include the objects\n",
    "            in the point cloud; those are handled separately.\n",
    "        context: The diagram context.  All positions in the context will be\n",
    "            held fixed *except* the gripper free body pose.\n",
    "        cloud: a PointCloud in world coordinates which represents candidate\n",
    "            grasps.\n",
    "        wsg_body_index: The body index of the gripper in plant.  If None, then\n",
    "            a body named \"body\" will be searched for in the plant.\n",
    "\n",
    "    Returns:\n",
    "        cost: The grasp cost\n",
    "\n",
    "    If adjust_X_G is True, then it also updates the gripper pose in the plant\n",
    "    context.\n",
    "    \"\"\"\n",
    "    plant = diagram.GetSubsystemByName(plant_system_name)\n",
    "    plant_context = plant.GetMyMutableContextFromRoot(context)\n",
    "    scene_graph = diagram.GetSubsystemByName(scene_graph_system_name)\n",
    "    scene_graph_context = scene_graph.GetMyMutableContextFromRoot(context)\n",
    "    if wsg_body_index:\n",
    "        wsg = plant.get_body(wsg_body_index)\n",
    "    else:\n",
    "        wsg = plant.GetBodyByName(\"body\")\n",
    "        wsg_body_index = wsg.index()\n",
    "\n",
    "    X_G = plant.GetFreeBodyPose(plant_context, wsg)\n",
    "\n",
    "    # Transform cloud into gripper frame\n",
    "    X_GW = X_G.inverse()\n",
    "    p_GC = X_GW @ cloud.xyzs()\n",
    "\n",
    "    # Crop to a region inside of the finger box.\n",
    "    crop_min = [-0.05, 0.1, -0.00625]\n",
    "    crop_max = [0.05, 0.1125, 0.00625]\n",
    "    indices = np.all(\n",
    "        (\n",
    "            crop_min[0] <= p_GC[0, :],\n",
    "            p_GC[0, :] <= crop_max[0],\n",
    "            crop_min[1] <= p_GC[1, :],\n",
    "            p_GC[1, :] <= crop_max[1],\n",
    "            crop_min[2] <= p_GC[2, :],\n",
    "            p_GC[2, :] <= crop_max[2],\n",
    "        ),\n",
    "        axis=0,\n",
    "    )\n",
    "\n",
    "    if meshcat_path:\n",
    "        pc = PointCloud(np.sum(indices))\n",
    "        pc.mutable_xyzs()[:] = cloud.xyzs()[:, indices]\n",
    "        meshcat.SetObject(\n",
    "            \"planning/points\", pc, rgba=Rgba(1.0, 0, 0), point_size=0.01\n",
    "        )\n",
    "\n",
    "    if adjust_X_G and np.sum(indices) > 0:\n",
    "        p_GC_x = p_GC[0, indices]\n",
    "        p_Gcenter_x = (p_GC_x.min() + p_GC_x.max()) / 2.0\n",
    "        X_G.set_translation(X_G @ np.array([p_Gcenter_x, 0, 0]))\n",
    "        plant.SetFreeBodyPose(plant_context, wsg, X_G)\n",
    "        X_GW = X_G.inverse()\n",
    "\n",
    "    query_object = scene_graph.get_query_output_port().Eval(\n",
    "        scene_graph_context\n",
    "    )\n",
    "\n",
    "    # Check collisions between the gripper and the sink\n",
    "    if query_object.HasCollisions():\n",
    "        cost = np.inf\n",
    "        if verbose:\n",
    "            print(\"Gripper is colliding with the sink!\\n\")\n",
    "            print(f\"cost: {cost}\")\n",
    "        return cost\n",
    "\n",
    "    # Check collisions between the gripper and the point cloud\n",
    "    # must be smaller than the margin used in the point cloud preprocessing.\n",
    "    margin = 0.0\n",
    "    for i in range(cloud.size()):\n",
    "        distances = query_object.ComputeSignedDistanceToPoint(\n",
    "            cloud.xyz(i), threshold=margin\n",
    "        )\n",
    "        if distances:\n",
    "            cost = np.inf\n",
    "            if verbose:\n",
    "                print(\"Gripper is colliding with the point cloud!\\n\")\n",
    "                print(f\"cost: {cost}\")\n",
    "            return cost\n",
    "\n",
    "    n_GC = X_GW.rotation().multiply(cloud.normals()[:, indices])\n",
    "\n",
    "    # Penalize deviation of the gripper from vertical.\n",
    "    # weight * -dot([0, 0, -1], R_G * [0, 1, 0]) = weight * R_G[2,1]\n",
    "    cost = 20.0 * X_G.rotation().matrix()[2, 1]\n",
    "\n",
    "    # Reward sum |dot product of normals with gripper x|^2\n",
    "    cost -= np.sum(n_GC[0, :] ** 2)\n",
    "    if verbose:\n",
    "        print(f\"cost: {cost}\")\n",
    "        print(f\"normal terms: {n_GC[0,:]**2}\")\n",
    "    return cost\n",
    "\n",
    "\n",
    "class ScoreSystem(LeafSystem):\n",
    "    def __init__(self, diagram, cloud, wsg_pose_index):\n",
    "        LeafSystem.__init__(self)\n",
    "        self._diagram = diagram\n",
    "        self._context = diagram.CreateDefaultContext()\n",
    "        self._plant = diagram.GetSubsystemByName(\"plant\")\n",
    "        self._plant_context = self._plant.GetMyMutableContextFromRoot(\n",
    "            self._context\n",
    "        )\n",
    "        wsg = self._plant.GetBodyByName(\"body\")\n",
    "        self._wsg_body_index = wsg.index()\n",
    "        self._wsg_pose_index = wsg_pose_index\n",
    "        self._cloud = cloud\n",
    "        self.DeclareAbstractInputPort(\n",
    "            \"body_poses\", AbstractValue.Make([RigidTransform()])\n",
    "        )\n",
    "        self.DeclareForcedPublishEvent(self.Publish)\n",
    "\n",
    "    def Publish(self, context):\n",
    "        X_WG = self.get_input_port(0).Eval(context)[self._wsg_pose_index]\n",
    "        self._plant.SetFreeBodyPose(\n",
    "            self._plant_context,\n",
    "            self._plant.get_body(self._wsg_body_index),\n",
    "            X_WG,\n",
    "        )\n",
    "        GraspCandidateCost(\n",
    "            self._diagram,\n",
    "            self._context,\n",
    "            self._cloud,\n",
    "            verbose=True,\n",
    "            meshcat_path=\"planning/cost\",\n",
    "        )\n",
    "        clear_output(wait=True)\n",
    "\n",
    "\n",
    "def process_point_cloud(diagram, context, cameras, bin_name):\n",
    "    plant = diagram.GetSubsystemByName(\"plant\")\n",
    "    plant_context = plant.GetMyContextFromRoot(context)\n",
    "\n",
    "    # Compute crop box.\n",
    "    bin_instance = plant.GetModelInstanceByName(bin_name)\n",
    "    bin_body = plant.GetBodyByName(\"bin_base\", bin_instance)\n",
    "    X_B = plant.EvalBodyPoseInWorld(plant_context, bin_body)\n",
    "    margin = 0.001  # only because simulation is perfect!\n",
    "    a = X_B.multiply(\n",
    "        [-0.22 + 0.025 + margin, -0.29 + 0.025 + margin, 0.015 + margin]\n",
    "    )\n",
    "    b = X_B.multiply([0.22 - 0.1 - margin, 0.29 - 0.025 - margin, 2.0])\n",
    "    crop_min = np.minimum(a, b)\n",
    "    crop_max = np.maximum(a, b)\n",
    "\n",
    "    pcd = []\n",
    "    for i in range(3):\n",
    "        cloud = diagram.GetOutputPort(f\"{cameras[i]}_point_cloud\").Eval(\n",
    "            context\n",
    "        )\n",
    "\n",
    "        # Crop to region of interest.\n",
    "        pcd.append(cloud.Crop(lower_xyz=crop_min, upper_xyz=crop_max))\n",
    "        # Estimate normals\n",
    "        pcd[i].EstimateNormals(radius=0.1, num_closest=30)\n",
    "\n",
    "        # Flip normals toward camera\n",
    "        camera = plant.GetModelInstanceByName(f\"camera{i}\")\n",
    "        body = plant.GetBodyByName(\"base\", camera)\n",
    "        X_C = plant.EvalBodyPoseInWorld(plant_context, body)\n",
    "        pcd[i].FlipNormalsTowardPoint(X_C.translation())\n",
    "\n",
    "    # Merge point clouds.\n",
    "    merged_pcd = Concatenate(pcd)\n",
    "\n",
    "    # Voxelize down-sample.  (Note that the normals still look reasonable)\n",
    "    return merged_pcd.VoxelizedDownSample(voxel_size=0.005)\n",
    "\n",
    "\n",
    "def make_environment_model(\n",
    "    directive=None, draw=False, rng=None, num_ycb_objects=0, bin_name=\"bin0\"\n",
    "):\n",
    "    # Make one model of the environment, but the robot only gets to see the sensor outputs.\n",
    "    if not directive:\n",
    "        directive = \"package://manipulation/two_bins_w_cameras.dmd.yaml\"\n",
    "\n",
    "    builder = DiagramBuilder()\n",
    "    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0005)\n",
    "    parser = Parser(plant)\n",
    "    ConfigureParser(parser)\n",
    "    parser.SetAutoRenaming(True)\n",
    "    parser.AddModelsFromUrl(directive)\n",
    "\n",
    "    for i in range(num_ycb_objects):\n",
    "        object_num = rng.integers(len(ycb))\n",
    "        parser.AddModelsFromUrl(\n",
    "            f\"package://manipulation/hydro/{ycb[object_num]}\"\n",
    "        )\n",
    "\n",
    "    plant.Finalize()\n",
    "    AddRgbdSensors(builder, plant, scene_graph)\n",
    "\n",
    "    if draw:\n",
    "        MeshcatVisualizer.AddToBuilder(\n",
    "            builder,\n",
    "            scene_graph,\n",
    "            meshcat,\n",
    "            MeshcatVisualizerParams(prefix=\"environment\"),\n",
    "        )\n",
    "\n",
    "    diagram = builder.Build()\n",
    "    context = diagram.CreateDefaultContext()\n",
    "\n",
    "    if num_ycb_objects > 0:\n",
    "        generator = RandomGenerator(rng.integers(1000))  # this is for c++\n",
    "        plant_context = plant.GetMyContextFromRoot(context)\n",
    "        bin_instance = plant.GetModelInstanceByName(bin_name)\n",
    "        bin_body = plant.GetBodyByName(\"bin_base\", bin_instance)\n",
    "        X_B = plant.EvalBodyPoseInWorld(plant_context, bin_body)\n",
    "        z = 0.2\n",
    "        for body_index in plant.GetFloatingBaseBodies():\n",
    "            tf = RigidTransform(\n",
    "                UniformlyRandomRotationMatrix(generator),\n",
    "                [rng.uniform(-0.15, 0.15), rng.uniform(-0.2, 0.2), z],\n",
    "            )\n",
    "            plant.SetFreeBodyPose(\n",
    "                plant_context, plant.get_body(body_index), X_B.multiply(tf)\n",
    "            )\n",
    "            z += 0.1\n",
    "\n",
    "        simulator = Simulator(diagram, context)\n",
    "        simulator.AdvanceTo(2.0 if running_as_notebook else 0.1)\n",
    "    elif draw:\n",
    "        diagram.ForcedPublish(context)\n",
    "\n",
    "    return diagram, context\n",
    "\n",
    "\n",
    "# Another diagram for the objects the robot \"knows about\": gripper, cameras, bins.  Think of this as the model in the robot's head.\n",
    "def make_internal_model():\n",
    "    builder = DiagramBuilder()\n",
    "    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)\n",
    "    parser = Parser(plant)\n",
    "    ConfigureParser(parser)\n",
    "    parser.AddModelsFromUrl(\"package://manipulation/clutter_planning.dmd.yaml\")\n",
    "    plant.Finalize()\n",
    "    return builder.Build()\n",
    "\n",
    "\n",
    "def grasp_score_inspector():\n",
    "    meshcat.Delete()\n",
    "    environment, environment_context = make_environment_model(\n",
    "        directive=\"package://manipulation/clutter_mustard.dmd.yaml\", draw=True\n",
    "    )\n",
    "\n",
    "    internal_model = make_internal_model()\n",
    "\n",
    "    # Finally, we'll build a diagram for running our visualization\n",
    "    builder = DiagramBuilder()\n",
    "    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)\n",
    "    parser = Parser(plant)\n",
    "    ConfigureParser(parser)\n",
    "    parser.AddModelsFromUrl(\"package://manipulation/clutter_planning.dmd.yaml\")\n",
    "    AddFloatingRpyJoint(\n",
    "        plant,\n",
    "        plant.GetFrameByName(\"body\"),\n",
    "        plant.GetModelInstanceByName(\"gripper\"),\n",
    "    )\n",
    "    plant.Finalize()\n",
    "\n",
    "    meshcat.DeleteAddedControls()\n",
    "    params = MeshcatVisualizerParams()\n",
    "    params.prefix = \"planning\"\n",
    "    visualizer = MeshcatVisualizer.AddToBuilder(\n",
    "        builder, scene_graph, meshcat, params\n",
    "    )\n",
    "\n",
    "    cloud = process_point_cloud(\n",
    "        environment,\n",
    "        environment_context,\n",
    "        [\"camera0\", \"camera1\", \"camera2\"],\n",
    "        \"bin0\",\n",
    "    )\n",
    "    meshcat.SetObject(\"planning/cloud\", cloud, point_size=0.003)\n",
    "\n",
    "    score = builder.AddSystem(\n",
    "        ScoreSystem(internal_model, cloud, plant.GetBodyByName(\"body\").index())\n",
    "    )\n",
    "    builder.Connect(plant.get_body_poses_output_port(), score.get_input_port())\n",
    "\n",
    "    lower_limit = [-1, -1, 0, -np.pi, -np.pi / 4.0, -np.pi / 4.0]\n",
    "    upper_limit = [1, 1, 1, 0, np.pi / 4.0, np.pi / 4.0]\n",
    "    q0 = [-0.05, -0.5, 0.25, -np.pi / 2.0, 0, 0]\n",
    "    default_interactive_timeout = None if running_as_notebook else 1.0\n",
    "    sliders = builder.AddSystem(\n",
    "        JointSliders(\n",
    "            meshcat,\n",
    "            plant,\n",
    "            initial_value=q0,\n",
    "            lower_limit=lower_limit,\n",
    "            upper_limit=upper_limit,\n",
    "            decrement_keycodes=[\n",
    "                \"KeyQ\",\n",
    "                \"KeyS\",\n",
    "                \"KeyA\",\n",
    "                \"KeyJ\",\n",
    "                \"KeyK\",\n",
    "                \"KeyU\",\n",
    "            ],\n",
    "            increment_keycodes=[\n",
    "                \"KeyE\",\n",
    "                \"KeyW\",\n",
    "                \"KeyD\",\n",
    "                \"KeyL\",\n",
    "                \"KeyI\",\n",
    "                \"KeyO\",\n",
    "            ],\n",
    "        )\n",
    "    )\n",
    "    diagram = builder.Build()\n",
    "    sliders.Run(diagram, default_interactive_timeout)\n",
    "    meshcat.DeleteAddedControls()\n",
    "\n",
    "\n",
    "grasp_score_inspector()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c53ac7db",
   "metadata": {},
   "source": [
    "# Generating grasp candidates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "39292548",
   "metadata": {},
   "outputs": [],
   "source": [
    "def draw_grasp_candidate(X_G, prefix=\"gripper\", draw_frames=True):\n",
    "    builder = DiagramBuilder()\n",
    "    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)\n",
    "    parser = Parser(plant)\n",
    "    ConfigureParser(parser)\n",
    "    parser.AddModelsFromUrl(\n",
    "        \"package://manipulation/schunk_wsg_50_welded_fingers.sdf\"\n",
    "    )\n",
    "    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(\"body\"), X_G)\n",
    "    plant.Finalize()\n",
    "\n",
    "    # frames_to_draw = {\"gripper\": {\"body\"}} if draw_frames else {}\n",
    "    params = MeshcatVisualizerParams()\n",
    "    params.prefix = prefix\n",
    "    params.delete_prefix_on_initialization_event = False\n",
    "    visualizer = MeshcatVisualizer.AddToBuilder(\n",
    "        builder, scene_graph, meshcat, params\n",
    "    )\n",
    "    diagram = builder.Build()\n",
    "    context = diagram.CreateDefaultContext()\n",
    "    diagram.ForcedPublish(context)\n",
    "\n",
    "\n",
    "def GenerateAntipodalGraspCandidate(\n",
    "    diagram,\n",
    "    context,\n",
    "    cloud,\n",
    "    rng,\n",
    "    wsg_body_index=None,\n",
    "    plant_system_name=\"plant\",\n",
    "    scene_graph_system_name=\"scene_graph\",\n",
    "):\n",
    "    \"\"\"\n",
    "    Picks a random point in the cloud, and aligns the robot finger with the normal of that pixel.\n",
    "    The rotation around the normal axis is drawn from a uniform distribution over [min_roll, max_roll].\n",
    "    Args:\n",
    "        diagram: A diagram containing a MultibodyPlant+SceneGraph that contains\n",
    "            a free body gripper and any obstacles in the environment that we\n",
    "            want to check collisions against. It should not include the objects\n",
    "            in the point cloud; those are handled separately.\n",
    "        context: The diagram context.  All positions in the context will be\n",
    "            held fixed *except* the gripper free body pose.\n",
    "        cloud: a PointCloud in world coordinates which represents candidate\n",
    "            grasps.\n",
    "        rng: a np.random.default_rng()\n",
    "        wsg_body_index: The body index of the gripper in plant.  If None, then\n",
    "            a body named \"body\" will be searched for in the plant.\n",
    "\n",
    "    Returns:\n",
    "        cost: The grasp cost\n",
    "        X_G: The grasp candidate\n",
    "    \"\"\"\n",
    "    plant = diagram.GetSubsystemByName(plant_system_name)\n",
    "    plant_context = plant.GetMyMutableContextFromRoot(context)\n",
    "    scene_graph = diagram.GetSubsystemByName(scene_graph_system_name)\n",
    "    scene_graph.GetMyMutableContextFromRoot(context)\n",
    "    if wsg_body_index:\n",
    "        wsg = plant.get_body(wsg_body_index)\n",
    "    else:\n",
    "        wsg = plant.GetBodyByName(\"body\")\n",
    "        wsg_body_index = wsg.index()\n",
    "\n",
    "    index = rng.integers(0, cloud.size() - 1)\n",
    "\n",
    "    # Use S for sample point/frame.\n",
    "    p_WS = cloud.xyz(index)\n",
    "    n_WS = cloud.normal(index)\n",
    "\n",
    "    assert np.isclose(\n",
    "        np.linalg.norm(n_WS), 1.0\n",
    "    ), f\"Normal has magnitude: {np.linalg.norm(n_WS)}\"\n",
    "\n",
    "    Gx = n_WS  # gripper x axis aligns with normal\n",
    "    # make orthonormal y axis, aligned with world down\n",
    "    y = np.array([0.0, 0.0, -1.0])\n",
    "    if np.abs(np.dot(y, Gx)) < 1e-6:\n",
    "        # normal was pointing straight down.  reject this sample.\n",
    "        return np.inf, None\n",
    "\n",
    "    Gy = y - np.dot(y, Gx) * Gx\n",
    "    Gz = np.cross(Gx, Gy)\n",
    "    R_WG = RotationMatrix(np.vstack((Gx, Gy, Gz)).T)\n",
    "    p_GS_G = [0.054 - 0.01, 0.10625, 0]\n",
    "\n",
    "    # Try orientations from the center out\n",
    "    min_roll = -np.pi / 3.0\n",
    "    max_roll = np.pi / 3.0\n",
    "    alpha = np.array([0.5, 0.65, 0.35, 0.8, 0.2, 1.0, 0.0])\n",
    "    for theta in min_roll + (max_roll - min_roll) * alpha:\n",
    "        # Rotate the object in the hand by a random rotation (around the normal).\n",
    "        R_WG2 = R_WG.multiply(RotationMatrix.MakeXRotation(theta))\n",
    "\n",
    "        # Use G for gripper frame.\n",
    "        p_SG_W = -R_WG2.multiply(p_GS_G)\n",
    "        p_WG = p_WS + p_SG_W\n",
    "\n",
    "        X_G = RigidTransform(R_WG2, p_WG)\n",
    "        plant.SetFreeBodyPose(plant_context, wsg, X_G)\n",
    "        cost = GraspCandidateCost(diagram, context, cloud, adjust_X_G=True)\n",
    "        X_G = plant.GetFreeBodyPose(plant_context, wsg)\n",
    "        if np.isfinite(cost):\n",
    "            return cost, X_G\n",
    "\n",
    "        # draw_grasp_candidate(X_G, f\"collision/{theta:.1f}\")\n",
    "\n",
    "    return np.inf, None\n",
    "\n",
    "\n",
    "def sample_grasps_example():\n",
    "    meshcat.Delete()\n",
    "    rng = np.random.default_rng()\n",
    "\n",
    "    environment, environment_context = make_environment_model(\n",
    "        rng=rng, num_ycb_objects=5, draw=False\n",
    "    )\n",
    "\n",
    "    internal_model = make_internal_model()\n",
    "    internal_model_context = internal_model.CreateDefaultContext()\n",
    "\n",
    "    # Finally a model for the visualization.\n",
    "    builder = DiagramBuilder()\n",
    "    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)\n",
    "    parser = Parser(plant)\n",
    "    ConfigureParser(parser)\n",
    "    parser.AddModelsFromUrl(\"package://manipulation/clutter_planning.dmd.yaml\")\n",
    "    plant.Finalize()\n",
    "\n",
    "    params = MeshcatVisualizerParams()\n",
    "    params.prefix = \"planning\"\n",
    "    visualizer = MeshcatVisualizer.AddToBuilder(\n",
    "        builder, scene_graph, meshcat, params\n",
    "    )\n",
    "    diagram = builder.Build()\n",
    "    context = diagram.CreateDefaultContext()\n",
    "    diagram.ForcedPublish(context)\n",
    "\n",
    "    # Hide the planning gripper\n",
    "    meshcat.SetProperty(\"planning/gripper\", \"visible\", False)\n",
    "\n",
    "    cloud = process_point_cloud(\n",
    "        environment,\n",
    "        environment_context,\n",
    "        [\"camera0\", \"camera1\", \"camera2\"],\n",
    "        \"bin0\",\n",
    "    )\n",
    "    meshcat.SetObject(\"planning/cloud\", cloud, point_size=0.003)\n",
    "\n",
    "    plant.GetMyContextFromRoot(context)\n",
    "    scene_graph.GetMyContextFromRoot(context)\n",
    "\n",
    "    costs = []\n",
    "    X_Gs = []\n",
    "    for i in range(100 if running_as_notebook else 2):\n",
    "        cost, X_G = GenerateAntipodalGraspCandidate(\n",
    "            internal_model, internal_model_context, cloud, rng\n",
    "        )\n",
    "        if np.isfinite(cost):\n",
    "            costs.append(cost)\n",
    "            X_Gs.append(X_G)\n",
    "\n",
    "    indices = np.asarray(costs).argsort()[:5]\n",
    "    for rank, index in enumerate(indices):\n",
    "        draw_grasp_candidate(\n",
    "            X_Gs[index], prefix=f\"{rank}th best\", draw_frames=False\n",
    "        )\n",
    "\n",
    "\n",
    "sample_grasps_example()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.10.7 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
